#include "./master_conn.h"
#include "../driver/can_bus.h"
#include "../conn/proto.h"

namespace service {

using namespace utils;
using namespace driver;
using namespace conn;

void MasterConn::dispatch(const Message &msg) {
    uint16_t req_id = msg.conn_info.request_id();
    for (uint8_t i = 0; i < FUTURE_COUNT; i ++) {
        RespFuture &fut = m_resp_futures[i];
        if (fut.request_id() == req_id) {
            fut.set(msg);
            return;
        }
    }
}

uint8_t MasterConn::get_seq() {
    if (m_seq > SEQ_MAX) {
        m_seq = 0;
    }
    uint8_t tmp = m_seq;
    m_seq ++;
    return tmp;
}

void MasterConn::reset_all() {
    for (uint8_t i = 0; i < FUTURE_COUNT; i ++) {
        m_resp_futures[i].free();
    }
}

conn::RespFuture* MasterConn::get_future(uint16_t req_id) {

    for (uint8_t i = 0; i < FUTURE_COUNT; i ++) {
        if (m_resp_futures[i].acquire(req_id)) {
            return m_resp_futures + i;
        }
    }
    return nullptr;
}

Error MasterConn::write(ConnInfo &conn_info, uint8_t len) {
    return canBus->write_ext_pkg_wait_ack(conn_info, m_write_buf, len);
}

Error MasterConn::query_task_info(TaskInfo &task_info, uint8_t dest, uint8_t task_id) {
    Message msg;
    Error err = simple_request(msg, dest, proto::REQ_GET_TASK_INFO, task_id);
    if (err) {
        goto finish;
    }
    err = msg.parse(task_info.req, task_info.seq);
finish:
    return err;
}

Error MasterConn::wait_for(const RespInfo &resp, Message &msg) {
    Error err;
    TaskInfo task_info;
    for (;;) {
        if (resp.ptr->wait(msg, 10 * 1000)) {
            break;
        }
        
        err = query_task_info(task_info, resp.dest);
        if (err) {
            break;
        }

        if (resp.req != task_info.req) {
            if (!resp.ptr->wait(msg, 1000)) {
                err = ec::RESP_TIMEOUT;
            }
            break;
        }
    }
    resp.ptr->free();
    return err;
}

}

